/**
 ******************************************************************************
 * @file    servo.c
 * @author  GEEKROS,  site:www.geekros.com
 ******************************************************************************
 */

#include "servo.h"

int Servo_Time = 2000;

Servo_Read_Write_Struct Servo_Read_Write;

static Servo_Struct Servo_Data[SERVO_LEN];

/*******************************************************************************
 * @funtion      : Return_Servo_Address
 * @description  : 舵机数据读取地址函数
 * @param         {int id} 舵机ID 可选值0~24
 * @return        {*}
 *******************************************************************************/
Servo_Struct *Return_Servo_Address(int id)
{
	return &Servo_Data[id - 1];
}

/*******************************************************************************
 * @funtion      : Servo_Init
 * @description  : 模块初始化，UART3作为总线舵机模块专用接口
 * @param         {int baud_rate} 波特率
 * @return        {*}
 *******************************************************************************/
void Servo_Init(int baud_rate)
{
	USART_InitTypeDef USART_InitStructure;
	GPIO_InitTypeDef GPIO_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;

	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);

	GPIO_PinAFConfig(GPIOD, GPIO_PinSource8, GPIO_AF_USART3);
	GPIO_PinAFConfig(GPIOD, GPIO_PinSource9, GPIO_AF_USART3);

	GPIO_StructInit(&GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;

	GPIO_Init(GPIOD, &GPIO_InitStructure);

	USART_DeInit(USART3);
	USART_StructInit(&USART_InitStructure);
	USART_InitStructure.USART_BaudRate = 1000000;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;

	USART_Init(USART3, &USART_InitStructure);
	USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);

	USART_Cmd(USART3, ENABLE);

	NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);

	for (int i = 1; i < SERVO_LEN; i++)
	{
		Servo_Data[i - 1].id = i;
	}
}

/*******************************************************************************
 * @funtion      : USART3_IRQHandler
 * @description  : 总线舵机模块中断函数，接收UART3数据
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void USART3_IRQHandler(void)
{
	uint8_t data;
	if (USART_GetFlagStatus(USART3, USART_IT_RXNE) != RESET)
	{
		data = USART_ReceiveData(USART3);
		Servo_Read_Write.Servo_Read_Buffer[Servo_Read_Write.Servo_Read_Len] = data;
		Servo_Read_Write.Servo_Read_Len = (Servo_Read_Write.Servo_Read_Len + 1) % SERVO_DATA_LEN;
	}
}

/*******************************************************************************
 * @funtion      : Servo_SCSToHost
 * @description  : 2个8位数组合为1个16位数
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
int Servo_SCSToHost(uint8_t ID, uint8_t DataL, uint8_t DataH)
{
	int Data;
	if (Servo_Data[ID].end)
	{
		Data = DataL;
		Data <<= 8;
		Data |= DataH;
	}
	else
	{
		Data = DataH;
		Data <<= 8;
		Data |= DataL;
	}
	return Data;
}

/*******************************************************************************
 * @funtion      : Servo_HostToSCS
 * @description  : 1个16位数拆分为2个8位数
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void Servo_HostToSCS(uint8_t ID, uint8_t *DataL, uint8_t *DataH, int Data)
{
	if (Servo_Data[ID - 1].end)
	{
		*DataL = (Data >> 8);
		*DataH = (Data & 0xff);
	}
	else
	{
		*DataH = (Data >> 8);
		*DataL = (Data & 0xff);
	}
}

/*******************************************************************************
 * @funtion      : Servo_Read_Flush
 * @description  : 刷新接收缓冲区
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void Servo_Read_Flush(void)
{
	Servo_Read_Write.Servo_Read_Len = 0;
	Servo_Read_Write.Servo_Read_Step = 0;
}

/*******************************************************************************
 * @funtion      : Servo_Write_Flush
 * @description  : 刷新发送缓冲区
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void Servo_Write_Flush(void)
{
	if (Servo_Read_Write.Servo_Write_Len)
	{
		Servo_Send(Servo_Read_Write.Servo_Write_Buffer, Servo_Read_Write.Servo_Write_Len);
		Servo_Read_Write.Servo_Write_Len = 0;
	}
}

/*******************************************************************************
 * @funtion      : Servo_Send
 * @description  : 发送舵机数据
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void Servo_Send(uint8_t *buf, uint8_t len)
{
	while (len--)
	{
		while ((USART3->SR & 0x40) == 0);
		USART_SendData(USART3, *buf++);
	}
}

/*******************************************************************************
 * @funtion      : Servo_WriteSCS、Servo_Write_Data
 * @description  : 发送前数据处理
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
int Servo_WriteSCS(unsigned char *nDat, int nLen)
{
	while (nLen--)
	{
		if (Servo_Read_Write.Servo_Write_Len < sizeof(Servo_Read_Write.Servo_Write_Buffer))
		{
			Servo_Read_Write.Servo_Write_Buffer[Servo_Read_Write.Servo_Write_Len] = *nDat;
			Servo_Read_Write.Servo_Write_Len++;
			nDat++;
		}
	}
	return Servo_Read_Write.Servo_Write_Len;
}

void Servo_Write_Data(uint8_t ID, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen, uint8_t Fun)
{
	uint8_t i;
	uint8_t msgLen = 2;
	uint8_t bBuf[6];
	uint8_t CheckSum = 0;
	bBuf[0] = 0xff;
	bBuf[1] = 0xff;
	bBuf[2] = ID;
	bBuf[4] = Fun;
	if (nDat)
	{
		msgLen += nLen + 1;
		bBuf[3] = msgLen;
		bBuf[5] = MemAddr;
		Servo_WriteSCS(bBuf, 6);
	}
	else
	{
		bBuf[3] = msgLen;
		Servo_WriteSCS(bBuf, 5);
	}
	CheckSum = ID + msgLen + Fun + MemAddr;
	if (nDat)
	{
		for (i = 0; i < nLen; i++)
		{
			CheckSum += nDat[i];
		}
		Servo_WriteSCS(nDat, nLen);
	}
	CheckSum = ~CheckSum;
	Servo_WriteSCS(&CheckSum, 1);
}

/*******************************************************************************
 * @funtion      : Servo_Read、Servo_ReadSCS、Servo_Check_Head、Servo_Read_Instructions
 * @description  : 读取前数据处理
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
int16_t Servo_Read(void)
{
	uint8_t Reback_Data;
	if (Servo_Read_Write.Servo_Read_Step != Servo_Read_Write.Servo_Read_Len)
	{
		Reback_Data = Servo_Read_Write.Servo_Read_Buffer[Servo_Read_Write.Servo_Read_Step];
		Servo_Read_Write.Servo_Read_Step = (Servo_Read_Write.Servo_Read_Step + 1) % SERVO_DATA_LEN;
		return Reback_Data;
	}
	else
	{
		return -1;
	}
}

int Servo_ReadSCS(unsigned char *nDat, int nLen)
{
	int Size = 0;
	int ComData;
	uint32_t t_begin = 0;
	uint32_t time_Count = 0;
	uint32_t t_user;
	uint32_t IOTimeOut = 20000;
	while (1)
	{
		ComData = Servo_Read();
		if (ComData != -1)
		{
			if (nDat)
			{
				nDat[Size] = ComData;
			}
			Size++;
			t_begin = 0;
		}
		if (Size >= nLen)
		{
			break;
		}
		time_Count++;
		t_user = time_Count - t_begin;
		if (t_user > IOTimeOut)
		{
			break;
		}
	}
	return Size;
}

int Servo_Check_Head(void)
{
	uint8_t bDat;
	uint8_t bBuf[2] = {0, 0};
	uint8_t Cnt = 0;
	while (1)
	{
		if (!Servo_ReadSCS(&bDat, 1))
		{
			if (!Servo_ReadSCS(&bDat, 1))
			{
				return 0;
			}
		}
		bBuf[1] = bBuf[0];
		bBuf[0] = bDat;
		if (bBuf[0] == 0xff && bBuf[1] == 0xff)
		{
			break;
		}
		Cnt++;
		if (Cnt > 10)
		{
			return 0;
		}
	}
	return 1;
}

int Servo_Read_Instructions(uint8_t id, uint8_t MemAddr, uint8_t *nData, uint8_t nLen)
{
	int Size;
	uint8_t bBuf[4];
	uint8_t calSum;
	uint8_t i;
	Servo_Read_Flush();
	Servo_Write_Data(id, MemAddr, &nLen, 1, SERVO_INST_READ);
	Servo_Write_Flush();

	delay_ms(1);

	if (!Servo_Check_Head())
	{
		if (Servo_ReadSCS(bBuf, 2) != 2)
		{
			return 0;
		}
	}

	if (Servo_ReadSCS(bBuf, 3) != 3)
	{
		return 0;
	}

	Size = Servo_ReadSCS(nData, nLen);
	if (Size != nLen)
	{
		return 0;
	}

	if (Servo_ReadSCS(bBuf + 3, 1) != 1)
	{
		return 0;
	}

	calSum = bBuf[0] + bBuf[1] + bBuf[2];
	for (i = 0; i < Size; i++)
	{
		calSum += nData[i];
	}
	calSum = ~calSum;
	if (calSum != bBuf[3])
	{
		return 0;
	}
	return Size;
}

/*******************************************************************************
 * @funtion      : Servo_Read_Byte
 * @description  : 读1字节
 * @param         {uint8_t ID} 舵机ID
 * @param         {uint8_t MemAddr} 内存表地址
 * @return        {*}
 *******************************************************************************/
int Servo_Read_Byte(uint8_t ID, uint8_t MemAddr)
{
	uint8_t bDat;
	int Size = Servo_Read_Instructions(ID, MemAddr, &bDat, 1);
	if (Size != 1)
	{
		return -1;
	}
	else
	{
		return bDat;
	}
}

/*******************************************************************************
 * @funtion      : Servo_Read_Word
 * @description  : 读2字节
 * @param         {uint8_t ID} 舵机ID
 * @param         {uint8_t MemAddr} 内存表地址
 * @return        {*}
 *******************************************************************************/
int Servo_Read_Word(uint8_t ID, uint8_t MemAddr)
{
	uint8_t nDat[2];
	int Size;
	uint16_t wDat;
	Size = Servo_Read_Instructions(ID, MemAddr, nDat, 2);
	if (Size != 2)
	{

		return -1;
	}
	wDat = Servo_SCSToHost(ID - 1, nDat[0], nDat[1]);
	return wDat;
}

/*******************************************************************************
 * @funtion      : Servo_Read_Position
 * @description  : 读取舵机位置信息
 * @param         {int id} 舵机ID
 * @return        {*}
 *******************************************************************************/
void Servo_Read_Position(int id)
{
	int Position = -1;
	Servo_Data[id - 1].end = 1;
	Position = Servo_Read_Word(id, SERVO_PRESENT_POSITION_L);
	Servo_Data[id - 1].position = Position;
}

/*******************************************************************************
 * @funtion      : Servo_Read_Speed
 * @description  : 读取舵机速度信息
 * @param         {int id} 舵机ID
 * @return        {*}
 *******************************************************************************/
void Servo_Read_Speed(int id)
{
	int Speed = -1;
	Speed = Servo_Read_Word(id, SERVO_PRESENT_SPEED_L);
	if (Speed != -1 && Speed & (1 << 15))
	{
		Speed = -(Speed & ~(1 << 15));
	}
	Servo_Data[id - 1].speed = Speed;
}

/*******************************************************************************
 * @funtion      : Servo_Read_Load
 * @description  : 读取舵机输出扭力信息
 * @param         {int id} 舵机ID
 * @return        {*}
 *******************************************************************************/
void Servo_Read_Load(int id)
{
	int Load = -1;
	Load = Servo_Read_Word(id, SERVO_PRESENT_LOAD_L);
	if (Load != -1 && Load & (1 << 10))
	{
		Load = -(Load & ~(1 << 10));
	}
	Servo_Data[id - 1].load = Load;
}

/*******************************************************************************
 * @funtion      : Servo_Read_IFLoad
 * @description  : 舵机是否受力
 * @param         {int id} 舵机ID
 * @return        {*}
 *******************************************************************************/
void Servo_Read_IFLoad(int id)
{
	int ifload = -1;
	ifload = Servo_Read_Byte(id, SERVO_TORQUE_ENABLE);
	Servo_Data[id - 1].ifload = ifload;
}

/*******************************************************************************
 * @funtion      : Servo_Read_Motion_State
 * @description  : 读取舵机运动状态信息
 * @param         {int id} 舵机ID
 * @return        {*}
 *******************************************************************************/
void Servo_Read_Motion_State(int id)
{
	int Motion_State = -1;
	Motion_State = Servo_Read_Byte(id, SERVO_MOVING);
	Servo_Data[id - 1].motion_state = Motion_State;
}

/*******************************************************************************
 * @funtion      : Servo_Read_Voltage
 * @description  : 读取舵机电压信息
 * @param         {int id} 舵机ID
 * @return        {*}
 *******************************************************************************/
void Servo_Read_Voltage(int id)
{
	int Voltage = -1;
	Voltage = Servo_Read_Byte(id, SERVO_PRESENT_VOLTAGE);
	Servo_Data[id - 1].voltage = Voltage;
}

/*******************************************************************************
 * @funtion      : Servo_Read_Current
 * @description  : 读取舵机电流信息
 * @param         {int id} 舵机ID
 * @return        {*}
 *******************************************************************************/
void Servo_Read_Current(int id)
{
	int Current = -1;
	Current = Servo_Read_Word(id, SERVO_PRESENT_CURRENT_L);
	if (Current != -1 && Current & (1 << 15))
	{
		Current = -(Current & ~(1 << 15));
	}
	Servo_Data[id - 1].current = Current;
}

/*******************************************************************************
 * @funtion      : Servo_Read_Temperature
 * @description  : 读取舵机温度信息
 * @param         {int id} 舵机ID
 * @return        {*}
 *******************************************************************************/
void Servo_Read_Temperature(int id)
{
	int Temperature = -1;
	Temperature = Servo_Read_Byte(id, SERVO_PRESENT_TEMPERATURE);
	Servo_Data[id - 1].temperature = Temperature;
}

/*******************************************************************************
 * @funtion      : Servo_Type
 * @description  : 设置舵机类型
 * @param         {int id} 舵机ID
 * @param         {int type} 舵机类型 可选值 1（飞特STS3032）、2（飞特SCS009）
 * @return        {*}
 *******************************************************************************/
void Servo_Type(int id, int type)
{
	if (id == 0)
	{
		for (int i = 0; i < SERVO_LEN; i++)
		{
			Servo_Data[i].type = (type == 1) ? SERVO_STS : SERVO_SCS;
			if (type == SERVO_SCS)
			{
				Servo_Data[i].end = 1;
			}
			if (type == SERVO_STS)
			{
				Servo_Data[i].end = 0;
			}
		}
	}
	else
	{
		if (id <= SERVO_LEN)
		{
			Servo_Data[id - 1].type = (type == 1) ? SERVO_STS : SERVO_SCS;
			if (type == SERVO_SCS)
			{
				Servo_Data[id - 1].end = 1;
			}
			if (type == SERVO_STS)
			{
				Servo_Data[id - 1].end = 0;
			}
		}
	}
}

/*******************************************************************************
 * @funtion      : Servo_Read_Data
 * @description  : 读取舵机数据
 * @param         {int id} 舵机ID
 * @return        {*}
 *******************************************************************************/
void Servo_Read_Data(int id)
{
	if (id == 0)
	{
		for (int i = 0; i < SERVO_LEN; i++)
		{
			Servo_Read_Voltage(i + 1);
			Servo_Read_Position(i + 1);
			Servo_Read_Temperature(i + 1);
			Servo_Read_Current(i + 1);
			Servo_Read_Motion_State(i + 1);
			Servo_Read_Load(i + 1);
			Servo_Read_Speed(i + 1);
		}
	}
	else
	{
		Servo_Read_Voltage(id);
		Servo_Read_Position(id);
		Servo_Read_Temperature(id);
		Servo_Read_Current(id);
		Servo_Read_Motion_State(id);
		Servo_Read_Load(id);
		Servo_Read_Speed(id);
	}
}

/*******************************************************************************
 * @funtion      : Servo_Write
 * @description  : 普通写指令
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void Servo_Write(uint8_t ID, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen)
{
	Servo_Read_Flush();
	Servo_Write_Data(ID, MemAddr, nDat, nLen, SERVO_INST_WRITE);
	Servo_Write_Flush();
}

/*******************************************************************************
 * @funtion      : Servo_Write_Position
 * @description  : 写入角度数据
 * @param         {uint8_t ID} 舵机ID
 * @param         {uint16_t Pos} 角度数据
 * @return        {*}
 *******************************************************************************/
void Servo_Write_Position(uint8_t ID, uint16_t Pos)
{
	uint8_t bBuf[2];
	Servo_HostToSCS(ID, bBuf + 0, bBuf + 1, Pos);
	Servo_Write(ID, SERVO_GOAL_POSITION_L, bBuf, 2);
}

/*******************************************************************************
 * @funtion      : Servo_Sync_Write
 * @description  : 同步写指令
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void Servo_Sync_Write(uint8_t ID[], uint8_t IDN, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen)
{
	uint8_t mesLen = ((nLen + 1) * IDN + 4);
	uint8_t Sum = 0;
	uint8_t bBuf[7];
	uint8_t i, j;

	bBuf[0] = 0xff;
	bBuf[1] = 0xff;
	bBuf[2] = 0xfe;
	bBuf[3] = mesLen;
	bBuf[4] = SERVO_INST_SYNC_WRITE;
	bBuf[5] = MemAddr;
	bBuf[6] = nLen;

	Servo_Read_Flush();
	Servo_WriteSCS(bBuf, 7);

	Sum = 0xfe + mesLen + SERVO_INST_SYNC_WRITE + MemAddr + nLen;

	for (i = 0; i < IDN; i++)
	{
		Servo_WriteSCS(&ID[i], 1);
		Servo_WriteSCS(nDat + i * nLen, nLen);
		Sum += ID[i];
		for (j = 0; j < nLen; j++)
		{
			Sum += nDat[i * nLen + j];
		}
	}

	Sum = ~Sum;
	Servo_WriteSCS(&Sum, 1);
	Servo_Write_Flush();
}

/*******************************************************************************
 * @funtion      : Servo_Sync_Write_Position
 * @description  : 控制多个舵机到达指定位置
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void Servo_Sync_Write_Position(uint8_t ID[], uint8_t IDN, uint16_t Position[], uint16_t Time[], uint16_t Speed[])
{
	uint8_t offbuf[32 * 6];
	uint8_t i;
	for (i = 0; i < IDN; i++)
	{
		uint16_t T, V;
		if (Time)
		{
			T = Time[i];
		}
		else
		{
			T = 0;
		}
		if (Speed)
		{
			V = Speed[i];
		}
		else
		{
			V = 0;
		}
		Servo_HostToSCS(ID[i], offbuf + i * 6 + 0, offbuf + i * 6 + 1, Position[i]);
		Servo_HostToSCS(ID[i], offbuf + i * 6 + 2, offbuf + i * 6 + 3, T);
		Servo_HostToSCS(ID[i], offbuf + i * 6 + 4, offbuf + i * 6 + 5, V);
	}
	Servo_Sync_Write(ID, IDN, SERVO_GOAL_POSITION_L, offbuf, 6);
}

/*******************************************************************************
 * @funtion      : Servo_Sync_Write_Load
 * @description  : 舵机力拒状态控制
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void Servo_Sync_Write_Load(uint8_t ID[], uint8_t IDN, uint8_t Load_Switch)
{
	uint8_t Sync_Data_Send[IDN];
	for (int i = 0; i < IDN; i++)
	{
		Sync_Data_Send[i] = Load_Switch;
	}
	Servo_Sync_Write(ID, IDN, SERVO_TORQUE_ENABLE, Sync_Data_Send, 1);
}

/*******************************************************************************
 * @funtion      : Servo_Sync_Write_Data
 * @description  : 同步写入地址数据
 * @param         {*}
 * @return        {*}
 *******************************************************************************/
void Servo_Sync_Write_Data(uint8_t ID[], uint8_t IDN, uint8_t Address, uint8_t Single_Data)
{
	uint8_t Sync_Data_Send[IDN];
	for (int i = 0; i < IDN; i++)
	{
		Sync_Data_Send[i] = Single_Data;
	}
	Servo_Sync_Write(ID, IDN, Address, Sync_Data_Send, 2);
}

/*******************************************************************************
 * @funtion      : Servo_Usb_Callback
 * @description  : 串口任务回调函数
 * @param         {char *type} 通讯协议类型
 * @param         {int id} 舵机ID 可选值0~24
 * @param         {int baud_rate} 波特率
 * @param         {int servo_type} 舵机类型 可选值 1（飞特STS3032）、2（飞特SCS009）
 * @return        {*}
 *******************************************************************************/
void Servo_Usb_Callback(char *type, int id, int baud_rate, int servo_type)
{
	if (memcmp(type, "servo-rate", 10) == 0)
	{
		Servo_Init(baud_rate);
		Servo_Type(id, servo_type);
	}
	if (memcmp(type, "servo-data", 10) == 0)
	{
		Usb_Write_Data("{\"type\":\"servo-data\",\"id\":\"%d\",\"pos\":\"%d\",\"voltage\":\"%d\",\"speed\":\"%d\",\"load\":\"%d\",\"temperature\":\"%d\",\"current\":\"%d\"}\r\n", Servo_Data[id - 1].id, Servo_Data[id - 1].position, Servo_Data[id - 1].voltage, Servo_Data[id - 1].speed, Servo_Data[id - 1].load, Servo_Data[id - 1].temperature, Servo_Data[id - 1].current);
	}
}
